//
// Created by 29787 on 2023/12/26.
//
#include "UPRE_Decision.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_GLOBAL_DEFINE.h"
#include "UPRE_PID.h"
#include "UPRE_Hand_Shank.h"
//�ٶ�ȫ�ֱ���
extern   bool flag_infrared_sensor;
extern   uint8_t  chassis[26];
extern   uint8_t  g_Auto_Num;
extern   uint8_t body_spin;
extern   float Angle;

struct   Hand_Shank Pack_receive={
        .flag_Ac=true,
        .flag_Dc=true,
        .num_lay=0,
        .num_fetch=0,
        .flag_infrared_sensor=true,
        .is_start_chassic=false,
        .Mode_execute=execute_null,
        .flag_stop_infrared_sensor=false,
};

struct Motor_Set Motor_Set_param={
        .posi_arml=0,
        .posi_armr=0,
        .vel_daj  =0,
        .vel_vesc_trans=0,
        .vesc_shoot=0
};

void Task_Decision() {
    if (Pack_receive.flag_infrared_sensor) {///���1s�ڷ�����������δ���ֶ�����
        if (flag_infrared_sensor)          ///��⵽����
        {
            Pack_receive.flag_stop_infrared_sensor=true;
        }
    }

    if (chassis[0] == 1 || chassis[1] == 1) {
        Pack_receive.Mode_execute = fetch_seeding;
        if (chassis[0] == 1)
            Pack_receive.num_fetch = 1;
        if (chassis[1] == 1)
            Pack_receive.num_fetch = 2;
    }

    if (chassis[2] == 1 || chassis[3] == 1 || chassis[4] == 1 || chassis[5] == 1) {
        Pack_receive.Mode_execute = lay_seedings;
        if (chassis[2] == 1)
            Pack_receive.num_lay = 1;
        if (chassis[3] == 1)
            Pack_receive.num_lay = 2;
        if (chassis[4] == 1)
            Pack_receive.num_lay = 3;
        if (chassis[5] == 1)
            Pack_receive.num_lay = 4;
    }

    if(chassis[10]==1)
    {
        Pack_receive.Air_open_stamp=0;
        chassis[10]=0;
    }

    if (Pack_receive.is_shoot_ball == 1) {      //high_speed_shoot
        Pack_receive.Mode_execute = shoot_ball;
    }

    if (Pack_receive.is_eat_ball == 1) {      //low_speed_eat
        Pack_receive.Mode_execute = eat_ball;
    }

    if (Pack_receive.is_accelerate == 1) {      //high_speed_shoot
        Pack_receive.Mode_execute = Accelerate;
    }

    if (Pack_receive.is_decelerate == 1) {      //low_speed_eat
        Pack_receive.Mode_execute = Dccelerate;
    }

    if(Pack_receive.flag_Air_open_stamp)
    {
        setAirCylinder_more(Air_1,Open_Air,5,OUT_3,OUT_4,OUT_5,OUT_6,OUT_7);
        Pack_receive.flag_Air_open_stamp=false;
    }

    if(Pack_receive.flag_body_spin)
    {
        if(body_spin)
        {
            Angle+=180.f;
            Pack_receive.flag_body_spin=false;
            Pack_receive.Body_spin_time_stamp=0;
        }
    }
    switch (Pack_receive.Mode_execute) {
        case fetch_seeding: {
            switch (Pack_receive.num_fetch) {
                case 1: {
                    up_pid();
                    setAirCylinder_more(Air_1, Close_Air, 2, AC_Claw_R1, AC_Claw_R2);       ///ץ
                    osDelay(1000);
                    Motor_Set_param.posi_armr = 440;
                    osDelay(1000);
                }
                    break;

                case 2: {
                    up_pid();
                    setAirCylinder_more(Air_1, Close_Air, 2, AC_Claw_L1, AC_Claw_L2);       ///ץ
                    osDelay(1000);
                    Motor_Set_param.posi_arml = 410;
                    osDelay(1000);
                }
                    break;
            }
            g_Auto_Num++;
            osDelay(200);          ///TODO �ص�: ����·��������������������һ��·����Ȼ���ٽ�������0��ʹ�ò����ٽ��жϣ�����һ�α�����
            chassis[0] = 0;         ///����û�ҵ���bug������ܵ�ʱ��·�� Ҳ���ܣ�����������1�ˣ����������������жϵ�ʱ����0����Ч�ģ�������Ҫ��������ߴ����ʱ����0
            chassis[1] = 0;         ///��ͬʱ��֤�Ǳ߲����ٸ�ֵ���������˸�СС����ʱ
            Pack_receive.Mode_execute = execute_null;       ///ִֻ��һ��
        }
            break;

        case lay_seedings: {
            switch (Pack_receive.num_lay) {
                case 1: {
                    down_pid();
                    Motor_Set_param.posi_armr = 100;
                    osDelay(500);
                    setAirCylinder_more(Air_1, Open_Air, 1, AC_Claw_R1);
                    up_pid();
                    osDelay(300);
                    Motor_Set_param.posi_armr = 200;
                }
                    break;

                case 2: {
                    down_pid();
                    Motor_Set_param.posi_armr = 100;
                    osDelay(500);
                    setAirCylinder_more(Air_1, Open_Air, 1, AC_Claw_R2);
                    osDelay(300);
                    Motor_Set_param.posi_armr = 0;
                }
                    break;

                case 3: {
                    down_pid();
                    Motor_Set_param.posi_arml = 100;
                    osDelay(500);
                    setAirCylinder_more(Air_1, Open_Air, 1, AC_Claw_L2);
                    up_pid();
                    osDelay(300);
                    Motor_Set_param.posi_arml = 200;
                }
                    break;

                case 4: {
                    down_pid();
                    Motor_Set_param.posi_arml = 100;
                    osDelay(500);
                    setAirCylinder_more(Air_1, Open_Air, 1, AC_Claw_L1);
                    osDelay(300);
                    Motor_Set_param.posi_arml = 0;
                }
                    break;
            }
            g_Auto_Num++;
            osDelay(200);
            chassis[2] = 0;
            chassis[3] = 0;
            chassis[4] = 0;
            chassis[5] = 0;
            Pack_receive.Mode_execute = execute_null;       ///ִֻ��һ�α��ÿ�
        }
            break;

        case eat_ball: {
            Motor_Set_param.vel_vesc_trans = 2500;
            Pack_receive.Mode_execute = execute_null;       ///ִֻ��һ��
        }
            break;

        case Accelerate: {
            if (Pack_receive.flag_Ac) {
                HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
                Motor_Set_param.vesc_shoot += 5000;
                Pack_receive.Ac_time_stamp  = 0;
                Pack_receive.flag_Ac = false;
            }
            Pack_receive.Mode_execute = execute_null;  ///TODO:λ�÷ǳ���������������Сʱ�������������������˳�������Ȼ���ܽ��������Ѿ�����ֵ
        }                                              ///TODO:���Ե���������֮���ǻ��ȥ��
            break;

        case Dccelerate: {
            if (Pack_receive.flag_Dc) {
                HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
                Motor_Set_param.vesc_shoot -= 5000;
                Pack_receive.Dc_time_stamp  = 0;
                Pack_receive.flag_Dc = false;
            }
            Pack_receive.Mode_execute = execute_null;       ///ִֻ��һ��
        }
            break;

        case shoot_ball: {
            Motor_Set_param.vel_vesc_trans = 2600;
            Pack_receive.Mode_execute = execute_null;       ///ִֻ��һ�α��ÿ�///�ų��������
            Pack_receive.flag_infrared_sensor = false;
            Pack_receive.infrared_sensor_time_stamp = 0;
        }
            break;


        case Motor_back: {
            Motor_Set_param.posi_armr = 0;
            Motor_Set_param.posi_arml = 0;
        }
            break;
    }
}

    void Time_Stamp()
    {
        Pack_receive.Ac_time_stamp += 1;
        Pack_receive.Dc_time_stamp += 1;
        Pack_receive.infrared_sensor_time_stamp +=1;
        Pack_receive.Body_spin_time_stamp+=1;

        if(Pack_receive.Body_spin_time_stamp>=500)
            Pack_receive.flag_body_spin=true;

        if(Pack_receive.flag_stop_infrared_sensor)
            Pack_receive.Stop_infrared_sensor_time_stamp+=1;

        if(Pack_receive.Stop_infrared_sensor_time_stamp>=300)
        {
            Pack_receive.Stop_infrared_sensor_time_stamp=0;
            Motor_Set_param.vel_vesc_trans=0;
            Pack_receive.flag_stop_infrared_sensor=false;
        }

        if(Pack_receive.is_start_chassic)
            Pack_receive.Air_open_stamp+=1;

        if (Pack_receive.infrared_sensor_time_stamp >= 500)     ///������������ʧ��3s
        {
            Pack_receive.flag_infrared_sensor = true;
        }
        if (Pack_receive.Ac_time_stamp >= 100)     ///���ټ�����300ms
        {
            Pack_receive.flag_Ac = true;
        }
        if (Pack_receive.Dc_time_stamp >= 100)     ///���ټ�����300ms
        {
            Pack_receive.flag_Dc = true;
        }
        if (Pack_receive.Air_open_stamp >= 300)     ///���ټ�����300ms
        {
            static bool num=true;
            if(num)
            {
                Pack_receive.flag_Air_open_stamp = true;
            }
            num=false;
        }
    }